#include "moto.h"


Planner sPlanner[3];



/// @brief  DJ电机数据清除函数
/// @param moto 
/// @param ID 
void Moto_stace_clear(pid_control *moto)
{
    moto->M3508_Check = 0;
    moto->round_cnt = 0;
    moto->all_angle = 0;
	moto->control_pos = 0;
	

}
void Moto_planning_Init(Planner *sPlanner,float acc,float dec)
{
 sPlanner->acc_percent= acc;
 sPlanner->dec_percent= dec; 
 sPlanner->Last_pos = 0;
 sPlanner->Last_set_pos = 0;

}

/// @brief *速度控制器
/// @param sPlanner 
/// @param pos 
/// @param min_speed 
/// @param max_speed 
/// @param set_pos 
/// @return 
float Moto_path_planning(Planner *sPlanner,float pos,float threshold,float min_speed,float max_speed,float set_pos )
{
 float speed = 0.0f;
  float Err = 0.0f;
  float Max_speed = 0.0f;
  pos = pos/360.0f;

   Max_speed = max_speed;
   /*******零点设置最大速度************/
  if(set_pos == 0)
  {
  return speed = max_speed;
  }
  /***********************************/
  if( sPlanner->Last_set_pos != set_pos) 
  {
	sPlanner->All_Err = fabs(set_pos - pos);
	sPlanner->Last_pos =  pos;
  }
  Err = fabs(pos - sPlanner->Last_pos);

  /*******短距离处理***********/
 if(sPlanner->All_Err <= threshold)
 {
  Max_speed = Max_speed * 0.30f ;
 }
  
  if(Err<sPlanner->acc_percent*sPlanner->All_Err)
  {
	speed=min_speed+(Max_speed-min_speed)*sin(Err/(sPlanner->acc_percent*sPlanner->All_Err)*pai*0.5f);
  }
  else if(Err>(1-sPlanner->dec_percent)*sPlanner->All_Err)
  {
   speed=min_speed +(Max_speed-min_speed)*sin(pai*0.5f+(Err-(1-sPlanner->dec_percent)*sPlanner->All_Err)/(sPlanner->dec_percent*sPlanner->All_Err)*pai*0.5f);
  }
  else 
  {
	speed = Max_speed;
  }
  
   sPlanner->Last_set_pos = set_pos;
   return fabs(speed);

}




